n_data = size(tau_ideal,2);
nb=size(tau_ideal,1);
n_data = 3000


cvx_begin sdp
    cvx_solver Mosek
    cvx_precision high
    variable X(nb*10)
    variable Bc(nb) %静摩擦力项
    variable B(nb)  %速度相关的粘滞摩擦力项
    expressions inertial_sdp_matrix(4,4,nb) % semidefinite  symmetric
    expressions C1(4,4) 
    expressions C2(4,4) 
    expressions C3(4,4) 
    expressions mj(nb)
    expressions hj(3,1,nb)
    expressions Ij(3,3,nb)
    expression error_norm1(n_data,1)
    
    joint_vel = dq;
    tau = tau_ideal;
    tau = tau_noise;
    X_ref = Para_vec;
    beta = 0.001;

    for i=1:n_data
%         error_norm1(i) = norm(K(:,:,i)*X-tau(:,i),2);
         error_norm1(i) = norm(K(:,:,i)*X + Bc.*sign(joint_vel(:,i)) + B.*joint_vel(:,i)-tau(:,i),2);
    end
%     error_norm1 = error_norm1/n_data;
    minimize sum(error_norm1)/n_data + beta*norm((X-X_ref)) 
    for j = 1:nb
        mj(j) = X((j-1)*10+1);
        hj(:,:,j) = X((j-1)*10+2:(j-1)*10+4);
        Ij(:,:,j) = [X((j-1)*10+5) X((j-1)*10+6) X((j-1)*10+7);
                    X((j-1)*10+6) X((j-1)*10+8) X((j-1)*10+9);
                    X((j-1)*10+7) X((j-1)*10+9) X((j-1)*10+10)];
       
        inertial_sdp_matrix(4,4,j) = X((j-1)*10+1);
        inertial_sdp_matrix(1:3,1:3,j) = 0.5*trace(Ij(:,:,j))*eye(3) - Ij(:,:,j);
        inertial_sdp_matrix(4,1:3,j) =  X((j-1)*10+2:(j-1)*10+4)';
        inertial_sdp_matrix(1:3,4,j) =  X((j-1)*10+2:(j-1)*10+4);
    end
    
    C1 = inertial_sdp_matrix(:,:,1);
    C2 = inertial_sdp_matrix(:,:,2);
    C3 = inertial_sdp_matrix(:,:,3);
    subject to

        C1 == semidefinite(4);
        C2 == semidefinite(4);
        C3 == semidefinite(4);

cvx_end